﻿namespace RLab.Visualization

open System
open System.Drawing
open System.Windows.Forms
open Microsoft.DirectX
open Microsoft.DirectX.Direct3D
open StillDesign.PhysX
open GeometryDefinitions

type ITool = 
    inherit System.IDisposable
    abstract Open  : unit->unit
    abstract Close : unit->unit    
    abstract End : Actor        

module Tool = 
    let hand (r: Renderer)  (e : PhysicEngine) = 
        
        let baseDim = V3(25.0f,15.f,60.f)
        let basePos = V3(800.0f,700.0f,0.f)
        let jawDim = V3(25.f,35.f,10.f)
        let rjawPos = V3(basePos.X,basePos.Y - baseDim.Y/2.f - jawDim.Y/2.f - 3.f, -baseDim.Z/2.f + jawDim.Z/2.f)
        let ljawPos = V3(basePos.X,basePos.Y - baseDim.Y/2.f - jawDim.Y/2.f - 3.f, baseDim.Z/2.f - jawDim.Z/2.f)
        let jawBox = r.CreateMeshBox(jawDim)
        let surfaceDim = V3(135.0f,66.f,170.f) * 5.f
        let surfaceMesh = r.CreateMeshBox(surfaceDim)
        let boxdim = V3(30.f,30.f,30.f)
        let box2dim = V3(25.f,25.f,25.f)
        let boxmesh = r.CreateMeshBox(boxdim) 
        let box2mesh = r.CreateMeshBox(box2dim)

        let toolBase = 
            { Name = "HANDBASE"
              Model = r.CreateMeshBox(baseDim)
              PhysicModel = baseDim,basePos
              Mass = 0.001f
              CenterOfMassOffset = O }
        let rJaw = 
            { Name = "RJAW"
              Model = jawBox
              PhysicModel = jawDim,rjawPos
              Mass = 0.001f
              CenterOfMassOffset = O }
        let lJaw = 
            { rJaw with
                PhysicModel = jawDim,ljawPos 
                Name = "LJAW" }
        //environment, refactor to aniother module
        let surface = 
            { Name = "SURFACE"
              Model = surfaceMesh
              PhysicModel = surfaceDim,V3(basePos.X,surfaceDim.Y/2.f,0.f)
              Mass = 10.f
              CenterOfMassOffset = O }
        let box = 
            { Name = "BOX"
              Model = boxmesh
              PhysicModel = boxdim,V3(basePos.X,surfaceDim.Y + boxdim.Y/2.f,basePos.Z)
              Mass = 0.0003f
              CenterOfMassOffset = O }
        let box2 = 
            { box with
                Model = boxmesh
                PhysicModel = box2dim,V3(basePos.X - 70.f,surfaceDim.Y + box2dim.Y/2.f,basePos.Z) }
        let toolBaseActor = PhysicEngine.createCubeActor e toolBase
        let rjawActor = PhysicEngine.createCubeActor2 e rJaw 1000.f 50.f
        let ljawActor = PhysicEngine.createCubeActor2 e lJaw 1000.f 50.f
        let surfaceActor = PhysicEngine.createCubeActor e surface
        let boxActor = PhysicEngine.createCubeActor e box
        let box2Actor = PhysicEngine.createCubeActor e box2
        PhysicEngine.createFixedJoint e null surfaceActor |> ignore

        let chainJoint =
             {Name = "TOOL_JOINT"
              Anchor = basePos + V3(0.f,-jawDim.Y,0.f)
              Axis = V3(0.f,0.f,1.f)
              Type = Prismatic(ForceDrive)}

        let chainJoint2 =
             {Name = "TOOL_JOINT"
              Anchor = basePos //+ V3(0.f,-jawDim.Y,0.f)
              Axis = V3(0.f,0.f,1.f)
              Type = Prismatic(ForceDrive)}

        let rJoint = PhysicEngine.createFixedJoint e toolBaseActor rjawActor //chainJoint
        let lJoint = PhysicEngine.createPrismaticJoint_TOOL e toolBaseActor ljawActor chainJoint 
        let lJoint2 = PhysicEngine.createPrismaticJoint_TOOL e toolBaseActor ljawActor chainJoint2 
             
        let drawableObjects = 
            [toolBaseActor,toolBase
             rjawActor,rJaw
             ljawActor,lJaw
             boxActor,box
             box2Actor,box2
             surfaceActor,surface]
        r.DrawScene.Add( 
            fun d ->
               drawableObjects |> List.iter (fun (a,p) ->               
                                               d.SetTransform(TransformType.World, Matrix.Translation(p.CenterOfMassOffset) * a.GlobalPose  * r.MeshScale  )
                                               p.Model.DrawSubset(0)))
        { new ITool with
          member x.Open() = 
                    PhysicEngine.movePrismaticTo_TOOL e 0.f 15.f (lJoint:?>D6Joint)
                    PhysicEngine.movePrismaticTo_TOOL e 0.f 15.f (lJoint2:?>D6Joint)
                    //PhysicEngine.movePrismaticTo_TOOL e 0.f 5.f (rJoint:?>D6Joint)
                    System.Threading.Thread.Sleep(2500)
          member x.Close() = 
                    PhysicEngine.movePrismaticTo_TOOL e -ljawPos.Z  15.f (lJoint:?>D6Joint)
                    PhysicEngine.movePrismaticTo_TOOL e -ljawPos.Z  15.f (lJoint2:?>D6Joint)
                    //PhysicEngine.movePrismaticTo_TOOL e -rjawPos.Z  5.f (rJoint:?>D6Joint)
                    System.Threading.Thread.Sleep(2500)
          member x.Dispose() = ()
          member x.End =  toolBaseActor }
    